#include <iostream>
#include <string>
#include <pcl/io/ply_io.h>  
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h> 
#include <pcl/registration/ndt.h> 
#include <pcl/point_types.h> 
#include <pcl/registration/icp.h> 
#include <pcl/visualization/pcl_visualizer.h> 
#include <pcl/console/time.h>   
#include <pcl/filters/voxel_grid.h> 
#include <pcl/common/transforms.h> 
#include <pcl/io/vtk_lib_io.h> 
#include <vector>
#include <pcl/point_cloud.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <pcl/keypoints/sift_keypoint.h>
#include <math.h>

using namespace std;


typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool get_point(const string& filename, vector<pcl::PointXYZ>& points_vector)
{
	ifstream fs;
	fs.open(filename.c_str(), ios::binary);
	if (!fs.is_open() || fs.fail())
	{
		fs.close();
		return (false);
	}

	string line;
	vector<string> st;

	while (!fs.eof())
	{
		getline(fs, line);
		// Ignore empty lines
		if (line.empty())
			continue;

		// Tokenize the line
		boost::trim(line);
		boost::split(st, line, boost::is_any_of("\t\r "), boost::token_compress_on);

		if (st.size() != 3)
			continue;

		pcl::PointXYZ point;
		point.x = double(atof(st[0].c_str()));
		point.y = double(atof(st[1].c_str()));
		point.z = double(atof(st[2].c_str()));
		points_vector.push_back(point);
	}
	fs.close();

	return (true);
}

void get_uvwsita(const Eigen::Matrix<double, 4, 4>& M, vector<double>& uvw,double& sita) {
	double t1 = M(2, 1) - M(1, 2);
	double t2 = M(0, 2) - M(2, 0);
	double t3 = M(1, 0) - M(0, 1);
	uvw[0] = 1 / sqrt(1 + (t2 * t2 + t3 * t3) / (t1 * t1));
	uvw[1] = uvw[0] * t2 / t1;
	uvw[2] = uvw[0] * t3 / t1;
	sita = acos((M(0, 0) - uvw[0] * uvw[0]) / (1 - uvw[0] * uvw[0])); // sita
	return;
}

void normalize(vector<double>& line) {
	double sum = sqrt(line[0] * line[0] + line[1] * line[1] + line[2] * line[2]);
	for (int i = 0; i < 3; ++i) {
		line[i] /= sum;
	}
}

void createLine(const vector<vector<PointT>>& refer_points,const vector<double>& uvw,vector<vector<double>>& lineVector) {
	for (int i = 0; i < refer_points[0].size(); ++i) {
		double a0 = refer_points[0][i].x - refer_points[1][i].x;
		double b0 = refer_points[0][i].y - refer_points[1][i].y;
		double c0 = refer_points[0][i].z - refer_points[1][i].z;
		lineVector[i][0] = (uvw[2] * b0 - c0 * uvw[1]) / (a0 * uvw[1] - uvw[0] * b0);
		lineVector[i][1] = (uvw[2] * a0 - c0 * uvw[0]) / (a0 * uvw[1] - uvw[0] * b0);
		lineVector[i][2] = 1;
		normalize(lineVector[i]);
	}
}

void iterate_points(const PointT& point1, const PointT& point2, const PointT& center, const vector<double>& uvw, const double t,PointT& point4,double& angle) {
	point4.x = uvw[0] * t + center.x;
	point4.y = uvw[1] * t + center.y; 
	point4.z = uvw[2] * t + center.z;

	vector<double> n1(3);
	vector<double> n2(3);
	double len1=0, len2=0;
	double n1_n2_dot = 0;
	for (int i = 0; i < 3; ++i) {
		n1[i] = point1.data[i] - point4.data[i];
		n2[i] = point2.data[i] - point4.data[i];
		len1 += n1[i];
		len2 += n2[i];
		n1_n2_dot += n1[i] * n2[i];
	}
	angle = acos(n1_n2_dot / (len1 * len2));
}

void computePoint(const vector<vector<PointT>>& refer_points, const vector<double>& uvw, const double sita, const PointT& point0, vector<PointT>& Rotation_points) {
	vector<vector<double>> lineVector(refer_points[0].size(), vector<double>(3));
	createLine(refer_points, uvw, lineVector);

	for (int i = 0; i < refer_points[0].size(); ++i) {
		const vector<double>& n0 = lineVector[i];
		double t = 0.2,h=0.1;                              //����������
		const PointT& point1 = refer_points[0][i];
		const PointT& point2 = refer_points[1][i];

		const PointT point3((point1.x + point2.x) / 2, (point1.y + point2.y) / 2, (point1.z + point2.z) / 2);  //�е�

		PointT point4;
		double angle;
		

		iterate_points(point1, point2, point3, uvw, t, point4, angle);
		if ((point4.x - point3.x) * (point0.x - point3.x) + (point4.y - point3.y) * (point0.y - point3.y) + (point4.z - point3.z) * (point0.z - point3.z)<0) {
			t = -t; h = -h;
			iterate_points(point1, point2, point3, uvw, t, point4, angle);
		}

		int count = 0;
		while (count < 10000 && abs(angle - sita)>0.005) {
			t += h;
			iterate_points(point1, point2, point3, uvw, t, point4, angle);
			count += 1;
		}
		Rotation_points[i] = point4;
	}
}

vector<double> linear_fitting_3D_points(const vector<PointT>& Rotation_points) {
	long double sum_x = 0, sum_y = 0, sum_z = 0, sum_xz = 0, sum_yz = 0, sum_zz = 0;
	for (auto point : Rotation_points) {
		sum_x += point.x;
		sum_y += point.y;
		sum_z += point.z;
		sum_xz += point.x * point.z;
		sum_yz += point.y * point.z;
		sum_zz += point.z * point.z;
	}
	double n = Rotation_points.size();
	double den = n * sum_zz - sum_z * sum_z;
	vector<double> Ans(4);  // k1,b1,k2,b2
	Ans[0] = (n * sum_xz - sum_x * sum_z) / den;
	Ans[1] = (sum_x - Ans[0] * sum_z) / n;
	Ans[2] = (n * sum_yz - sum_y * sum_z) / den;
	Ans[3] = (sum_y - Ans[2] * sum_z) / n;

	return Ans;
}


int main(int argc, char* argv[])
{
	if (argc != 22)
	{
		printf("{\"success\":false}");
		return (0);
	}

	Eigen::Matrix4d transformation_matrix;
	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			transformation_matrix(i, j) = atof(argv[4 * i + j + 6]);
		}
	}

	vector<vector<PointT>> refer_points(2, vector<PointT>());
	
	if (!get_point(argv[1], refer_points[0]) || !(get_point(argv[2], refer_points[1]))) {
		printf("{\"success\":false}");
		return (0);
	}

	vector<double> uvw(3); //���ݱ任����õ�����ת��
	double sita;
	get_uvwsita(transformation_matrix, uvw, sita);

	PointT point0(double(atof(argv[4])), double(atof(argv[5])), double(atof(argv[6]))); //��ͻ�Ĳο��� Ϊ���� �ýǶȲ��ҵ��ʱ�� �������������

	vector<PointT> Rotation_points(refer_points[0].size());
	computePoint(refer_points, uvw, sita, point0, Rotation_points);
	vector<double> k1_k2_b1_b2 = linear_fitting_3D_points(Rotation_points);

	printf("{");
	printf("\"success\":true");

	printf(",\"axis\":[%6.3f,%6.3f,%6.3f,%6.3f]",k1_k2_b1_b2[0],k1_k2_b1_b2[1],k1_k2_b1_b2[2],k1_k2_b1_b2[3]);

	printf("}\n");

}